/*
 * Copyright 2025 The Authors
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 *@file    : frame.h
 *@brief   : frame
 *@authors : zhanglei
 *@version : v1.0
 *@data    : 2025/9/14
 *
 */

#ifndef FRAME_H
#define FRAME_H

#include "common/state.h"
#include "preintegration/imu_preintegration.h"
#include "preintegration/wheel_preintegration.h"
#include "feature/feature_database.h"

namespace frame {

using namespace preintegration;
using namespace feature;

class Frame {
public:
    /**
     * @brief Construct a new landmark object
     *
     */
    Frame(std::shared_ptr<FeatureDatabase> feature_database_ptr, const Eigen::Matrix3d& intrinsic_matrix, const Eigen::Matrix4d& extrinsic_matrix, const uint8_t& camera_id);
    void InsertSlidingWindowData(const uint64_t& frame_id, const State& state, const std::shared_ptr<ImuPreIntegration>& imu_preint_ptr, const std::shared_ptr<WheelPreIntegration>& wheel_preint_ptr, const Eigen::Vector2d& vel_world);
    void MarginalizationOldestFrame();
    uint8_t GetSlidingWindowSize();
    uint64_t GetFrameId();
    std::deque<State> state_queue_;  
    std::deque<std::shared_ptr<ImuPreIntegration>> imu_preintegration_ptr_queue_;
    std::deque<std::shared_ptr<WheelPreIntegration>> wheel_preintegration_ptr_queue_;
    std::shared_ptr<FeatureDatabase> feature_database_ptr_;
    std::deque<uint64_t> frame_ids_queue_;
    std::deque<Eigen::Vector2d> velocity_queue_;
private:
    uint64_t frame_id_; 
    uint8_t sliding_window_size_; 
    const Eigen::Matrix3d intrinsic_matrix_; 
    const Eigen::Matrix4d extrinsic_matrix_;

    uint8_t camera_id_;
};

}
#endif // TRIANGULATE_H